#!/bin/bash
# THIS IS A GENERATED FILE, NOT RECOMMENDED TO EDIT.

function log() {
  logger -s -p user.$1 ${@:2}
}
echo "sunrise" | sudo -S usermod -a -G i2c sunrise
log info "myrobot: Using workspace setup file /home/sunrise/catkin_ws/devel/setup.bash"
source /home/sunrise/catkin_ws/devel/setup.bash
JOB_FOLDER=/etc/ros/noetic/myrobot.d

log_path="/tmp"
if [[ ! -d $log_path ]]; then
  CREATED_LOGDIR=true
  trap 'CREATED_LOGDIR=false' ERR
    log warn "myrobot: The log directory you specified \"$log_path\" does not exist. Attempting to create."
    mkdir -p $log_path 2>/dev/null
    chown sunrise:sunrise $log_path 2>/dev/null
    chmod ug+wr $log_path 2>/dev/null
  trap - ERR
  # if log_path could not be created, default to tmp
  if [[ $CREATED_LOGDIR == false ]]; then
    log warn "myrobot: The log directory you specified \"$log_path\" cannot be created. Defaulting to \"/tmp\"!"
    log_path="/tmp"
  fi
fi

export ROS_HOSTNAME=$(hostname)

export ROS_MASTER_URI=http://127.0.0.1:11311
export ROS_HOME=${ROS_HOME:=$(echo ~sunrise)/.ros}
export ROS_LOG_DIR=$log_path

log info "myrobot: Launching ROS_HOSTNAME=$ROS_HOSTNAME, ROS_IP=$ROS_IP, ROS_MASTER_URI=$ROS_MASTER_URI, ROS_HOME=$ROS_HOME, ROS_LOG_DIR=$log_path"

# If xacro files are present in job folder, generate and expand an amalgamated urdf.
XACRO_FILENAME=$log_path/myrobot.xacro
XACRO_ROBOT_NAME=$(echo "myrobot" | cut -d- -f1)
rosrun robot_upstart mkxacro $JOB_FOLDER $XACRO_ROBOT_NAME > $XACRO_FILENAME
if [[ "$?" == "0" ]]; then
  URDF_FILENAME=$log_path/myrobot.urdf
  rosrun xacro xacro $XACRO_FILENAME -o $URDF_FILENAME
  if [[ "$?" == "0" ]]; then
    log info "myrobot: Generated URDF: $URDF_FILENAME"
  else
    log warn "myrobot: URDF macro expansion failure. Robot description will not function."
  fi
  export ROBOT_URDF_FILENAME=$URDF_FILENAME
fi

# Assemble amalgamated launchfile.
LAUNCH_FILENAME=$log_path/myrobot.launch
rosrun robot_upstart mklaunch $JOB_FOLDER > $LAUNCH_FILENAME
if [[ "$?" != "0" ]]; then
  log err "myrobot: Unable to generate amalgamated launchfile."
  exit 1
fi
log info "myrobot: Generated launchfile: $LAUNCH_FILENAME"

# Warn and exit if setpriv is missing from the system.
which setpriv > /dev/null
if [ "$?" != "0" ]; then
  log err "myrobot: Can't launch as unprivileged user without setpriv. Please install the setpriv package."
  exit 1
fi

# Punch it.
setpriv --reuid sunrise --regid sunrise --init-groups roslaunch $LAUNCH_FILENAME &
PID=$!

log info "myrobot: Started roslaunch as background process, PID $PID, ROS_LOG_DIR=$ROS_LOG_DIR"
echo "$PID" > $log_path/myrobot.pid

wait "$PID"
